
// import haxe.ds.Array;
// import oimo.common.Setting;
// import oimo.dynamics.constraint.info.joint.JointSolverInfo;
// import oimo.dynamics.constraint.info.joint.JointSolverInfoRow;

import { BoundaryBuildInfo } from "./BoundaryBuildInfo";
import { JointSolverInfo } from "../../info/joint/JointSolverInfo";
import { MassMatrix } from "./MassMatrix";
import { JointSolverInfoRow } from "../../info/joint/JointSolverInfoRow";
import { Setting } from "../../../../common/Setting";

/**
 * numberernal class
 */

export class Boundary {
	// number of impulses which are at lower or upper limits
	numBounded: number;
	// indices of impulses which are at lower or upper limits
	iBounded: Array<number>;
	// -1: at lower, 1: at upper
	signs: Array<number>;

	// number of impulses which are not at limits
	numUnbounded: number;
	// indices of impulses which are not at lower or upper limits
	iUnbounded: Array<number>;

	// used for impulse computation:
	//     impulse = massMatrix * b
	b: Array<number>;

	// the id of mass matrix
	matrixId: number;

	constructor(maxRows: number) {
		this.iBounded = new Array<number>(maxRows);
		this.iUnbounded = new Array<number>(maxRows);
		this.signs = new Array<number>(maxRows);
		this.b = new Array<number>(maxRows);
		this.numBounded = 0;
		this.numUnbounded = 0;
		this.matrixId = 0;
	}

	public init(buildInfo: BoundaryBuildInfo): void {
		// copy bounded part
		this.numBounded = buildInfo.numBounded;
		for (let i = 0; i < this.numBounded; ++i) {
			this.iBounded[i] = buildInfo.iBounded[i];
			this.signs[i] = buildInfo.signs[i];
		}

		// copy unbounded part and compute matrix id
		this.numUnbounded = buildInfo.numUnbounded;
		this.matrixId = 0;
		for (let i = 0; i < this.numUnbounded; ++i) {
			var idx: number = buildInfo.iUnbounded[i];
			this.iUnbounded[i] = idx;
			this.matrixId |= 1 << idx;
		}
	}

	public computeImpulses(info: JointSolverInfo, mass: MassMatrix, relVels: Array<number>, impulses: Array<number>, dImpulses: Array<number>, impulseFactor: number, noCheck: boolean): boolean {
		// b = rhs - relV - cfm * totalImpulse
		for (let i = 0; i < this.numUnbounded; ++i) {
			var idx: number = this.iUnbounded[i];
			var row: JointSolverInfoRow = info.rows[idx];
			var relVel: number = relVels[idx];
			this.b[idx] = row.rhs * impulseFactor - relVel - row.cfm * impulses[idx];
		}

		// bounded part
		var invMassWithoutCfm: Array<Array<number>> = mass._invMassWithoutCfm;
		for (let i = 0; i < this.numBounded; ++i) {
			var idx: number = this.iBounded[i];
			var sign: number = this.signs[i];
			var row: JointSolverInfoRow = info.rows[idx];
			var oldImpulse: number = impulses[idx];
			var impulse: number = sign < 0 ? row.minImpulse : sign > 0 ? row.maxImpulse : 0;
			var dImpulse: number = impulse - oldImpulse;
			dImpulses[idx] = dImpulse;

			// update relative velocity for unbounded part
			if (dImpulse != 0) {
				for (let j = 0; j < this.numUnbounded; ++j) {
					var idx2: number = this.iUnbounded[j];

					// delta of idx2'th relative velocity
					var dRelVel: number = invMassWithoutCfm[idx][idx2] * dImpulse;

					this.b[idx2] -= dRelVel;
				}
			}
		}

		var massMatrix: Array<Array<number>> = mass.getSubmatrix(this.iUnbounded, this.numUnbounded);
		var ok: boolean = true;

		// unbounded part
		for (let i = 0; i < this.numUnbounded; ++i) {
			var idx: number = this.iUnbounded[i];
			var row: JointSolverInfoRow = info.rows[idx];
			var oldImpulse: number = impulses[idx];

			var impulse: number = oldImpulse;

			// compute unbounded impulse (massMatrix * b)
			for (let j = 0; j < this.numUnbounded; ++j) {
				var idx2: number = this.iUnbounded[j];
				impulse += this.b[idx2] * massMatrix[i][j];
			}

			if (impulse < row.minImpulse - Setting.directMlcpSolverEps || impulse > row.maxImpulse + Setting.directMlcpSolverEps) {
				// we assumed that `impulse` holds `minImpulse <= impulse <= maxImpulse`, but actually
				// not. This boundary for the MLCP is not the answer of it.
				ok = false;
				break;
			}

			dImpulses[idx] = impulse - oldImpulse;
		}

		if (noCheck) return true;

		if (!ok) return false;

		// check if the impulses fulfill complementarity constraints
		for (let i = 0; i < this.numBounded; ++i) {
			var idx: number = this.iBounded[i];
			var row: JointSolverInfoRow = info.rows[idx];
			var sign: number = this.signs[i];
			var error: number = 0;

			var newImpulse: number = impulses[idx] + dImpulses[idx];
			var relVel: number = relVels[idx];

			// `relVel` is the relative velocity BEFORE we apply the delta impulses,
			// so we should update `relVel` so that it shows the relative velocity
			// AFTER we applied the delta impulse.
			for (let j = 0; j < info.numRows; ++j) {
				relVel += invMassWithoutCfm[idx][j] * dImpulses[j];
			}

			error = row.rhs * impulseFactor - relVel - row.cfm * newImpulse;
			// complementarity constraint: check if we added too large impulses
			//     sign < 0 => error <= 0
			//     sign > 0 => error >= 0
			if (sign < 0 && error > Setting.directMlcpSolverEps || sign > 0 && error < -Setting.directMlcpSolverEps) {
				ok = false; // the result is not feasible
				break;
			}
		}

		return ok;
	}

	/*
	public dump():void {
		trace("\nboundary info --------------------------------------");
		trace("bounded indices:");
		for (let i = 0; i < this.numBounded; ++i) {
			var idx:number = iBounded[i];
			trace("  " + idx + ", sign: " + signs[i]);
		}
		trace("unbounded indices:");
		for (let i = 0; i < this.numUnbounded; ++i) {
			var idx:number = iUnbounded[i];
			trace("  " + idx);
		}
		trace("mass matrix index: " + matrixId);
		trace("boundary info end ----------------------------------\n");
	}
	*/

}
